#include <QDebug>
#include <QPainter>

#include "QwController.h"

QwController::QwController() : QWidget()
{
    ctrlg = ctrld = 0;

    joy_x = joy_y = 0;
    max_x = max_y = 240;

    m_cmd_left = m_cmd_right = 0;

    setFixedSize( max_x, max_y );
    setWindowTitle( "Robot controller");
}

void QwController::calculCommand()
{
    int moyx, moyy, vmg, vmd, minmax, comg, comd, stop;

    ////debut filtrage
    moyx=joy_x/2;
    moyy=-joy_y/2;

    if(moyy>=0)
    {
	vmg=(abs(moyx+moyy));
	vmd=(abs(moyx-moyy));
    }
    else if (moyy<0)
    {
	vmg=(abs(moyx-moyy));
	vmd=(abs(moyx+moyy));
    }

    if (ctrlg==128) minmax=40;
    else minmax=60;

    if (moyy>0)
    {
	    if (moyx>=0)
	    {
		    if(vmg>minmax) vmg=minmax;
		    if(moyx>moyy) vmd=0;
	    }

	    if (moyx<=0) //
	    {
		    if(abs(moyx)>moyy) vmg=0;
		    if(vmd>minmax) vmd=minmax;
	    }
	    if (ctrlg==128) if(vmg>minmax)vmg=minmax;
	    if (ctrlg==128) if(vmd>minmax)vmd=minmax;
	    comg=ctrlg+64+vmg;comd=ctrld+64+vmd;					//Av
    }
    else if (moyy<0)
    {
    if (moyx>=0)
    {
	    if(vmg>minmax) vmg=minmax;
	    if(moyx>abs(moyy)) vmd=0;
    }

    if (moyx<=0)  //
    {
	    if(vmd>minmax) vmd=minmax;
	    if(moyx<moyy) vmg=0;
    }
    if (ctrlg==128) if(vmg>minmax)vmg=minmax;
    if (ctrlg==128) if(vmd>minmax)vmd=minmax;
    comg=ctrlg+vmg;comd=ctrld+vmd; 							//Ar
    }

    else
    {
    if ((moyy==0)&&(moyx<0)) {comg=ctrlg+vmg;comd=ctrld+64+vmd;} 			//Gpivot
    if ((moyy==0)&&(moyx>0)) {comg=ctrlg+64+vmg;comd=ctrld+vmd;}			//Dpivot
    if ((moyx==0)&&(moyy==0)) {stop=1;comg=0;comd=0;vmg=0;vmd=0;}

    //Fin filtrage
    }

    m_cmd_left = comg;
    m_cmd_right = comd;

    //qDebug() << "joy x" << joy_x << "joy y" << -joy_y << "cmd left" << m_cmd_left << "cmd right" << m_cmd_right;
}

void QwController::paintEvent( QPaintEvent *event )
{
    QPainter painter( this );
    QPen pen;

    pen.setWidth( 10 );
    pen.setColor( Qt::black );
    painter.setPen( pen );

    painter.drawPoint ( QPointF( joy_x + max_x/2, joy_y + max_y/2 ) );

    pen.setWidth( 3 );
    pen.setColor( Qt::black );
    painter.setPen( pen );

    painter.drawPoint ( QPointF( max_x/2, max_y/2 ) );
}

void QwController::doUpdate()
{
    int tmp_cmd_left = m_cmd_left;
    int tmp_cmd_right = m_cmd_right;

    update(); // redraw the virtual joystick
    calculCommand();

    if ( tmp_cmd_left != m_cmd_left || tmp_cmd_right != m_cmd_right )
	emit newCommand( m_cmd_left, m_cmd_right );
}

void QwController::keyPressEvent( QKeyEvent * event )
{
    switch (event->key())
    {
	case Qt::Key_Up:
	    joy_x = 0; joy_y = -60;
	    doUpdate();
	    break;
	case Qt::Key_Down:
	    joy_x = 0; joy_y = 60;
	    doUpdate();
	    break;
	case Qt::Key_Right:
	    joy_x = 60; joy_y = 0;
	    doUpdate();
	    break;
    case Qt::Key_Left:
	    joy_x = -60; joy_y = 0;
	    doUpdate();
	    break;
    }
}

void QwController::keyReleaseEvent( QKeyEvent * event )
{
    if ( event->isAutoRepeat() )
    {
	event->ignore();
	return;
    }

    if ( event->key() == Qt::Key_Up || event->key() == Qt::Key_Down ||
	 event->key() == Qt::Key_Left || event->key() == Qt::Key_Right )
    {
	joy_x = joy_y = 0;

	doUpdate();
    }
}

void QwController::mousePressEvent ( QMouseEvent * event )
{
    int px = event->pos().x();
    int py = event->pos().y();

    if ( px < 0 ) px = 0;
    if ( px >= max_x ) px = max_x - 1;
    if ( py < 0 ) py = 0;
    if ( py >= max_y ) py = max_y - 1;

    joy_x = px - max_x / 2;
    joy_y = py - max_y / 2;

    doUpdate();
}

void QwController::mouseMoveEvent ( QMouseEvent * event )
{
    int px = event->pos().x();
    int py = event->pos().y();

    if ( px < 0 ) px = 0;
    if ( px >= max_x ) px = max_x - 1;
    if ( py < 0 ) py = 0;
    if ( py >= max_y ) py = max_y - 1;

    joy_x = px - max_x / 2;
    joy_y = py - max_y / 2;

    doUpdate();
}

void QwController::mouseReleaseEvent( QMouseEvent * event )
{
    joy_x = joy_y = 0;

    doUpdate();
}

